from ShapePhysics import *
from plot_polygon import plot_polygon, plot_polygon_ls
from plot_polygon import get_sorted_points


class EPAClass(object):
    def __init__(self, aa: ShapeBase, bb: ShapeBase, simp: Simplex):
        self.aa = aa
        self.bb = bb
        self.simp = simp
        self.poly = EPAPolygon(simp)

        self.used_support_points = []       # 被使用过的support点

        self.N = aa.shape[0] * bb.shape[0] - len(simp)  # 最大循环次数为 点集数量 - 单纯形顶点数

    def get_furthest_point(self, d, stype: int, used_points: list):
        # 根据stype判断对哪个形状进行计算
        if stype:
            shape = self.aa
        else:
            shape = self.bb
        index_max = shape.dot(d).argmax()
        # try:
        #     index_max = shape.dot(d).argmax()
        # except:
        #     shape.dot(d).argmax()
        #     print(index_max)
        ret = shape[index_max]

        # shape = np.delete(shape, index_max, axis=0)     # 删除用过的点, 但函数内部变量暂时无法实现
        #
        # if stype:
        #     self.aa = shape
        # else:
        #     self.bb = shape

        return ret

    def support(self, d):
        # 获得支持向量
        if not isinstance(d, np.ndarray):
            d = np.array(d)

        # 这里要避免重复, 暂未实现
        p1 = self.get_furthest_point(d, 0, used_points=[])
        p2 = self.get_furthest_point(-d, 1, used_points=[])
        p3 = p1 - p2

        if 0:       # 未能解决陷入循环的问题, 陷入死循环. 原因应该是方向d循环导致的support点也循环
            edges = self.poly.edges
            show_ls(edges)
            p3 in edges
            poly = self.poly
            poly.judge_contains_p(p3)
        return p3

    def get_penetrate_vector(self):
        """
        得到穿透向量
        """
        poly = self.poly
        aa, bb = self.aa, self.bb
        N = self.N

        i = 0
        while True:
            i += 1

            # 找到距离原点最近的边
            e = poly.find_closest_edge()

            # 沿着边的法线方向，尝试找一个新的support点
            # p = support(aa, bb, e.normal)
            p = self.support(e.normal)      # 这里得想办法避免重复!!!

            # 无法找到能够跨越该边的support点了, 也就是说，该边就是差集最近边
            # new_distance = e.normal.dot(p)
            new_distance = p.dot(e.normal)
            if abs(new_distance - e.distance) < args.epsilon:
                # can't find new point in the e.norm's direction
                ret = e.normal * e.distance
                break

            # if poly.judge_contains_p(p):      # 这里应判断边, 但也不太好弄... 总之还是根据support点不要重复来解决问题最好.
            #     msg = '=== 新的点已在多边形中, 将陷入无限循环...'
            #     warnings.warn(msg)
            #     ret = e.normal * e.distance
            #     break

            if i > N:
                # 遍历后发现第一个就是最大的穿透向量了
                ret = e.normal * e.distance
                msg = f'--- 程序遍历了所有顶点, 没找到适合的穿透向量! 只能返回最后一个了...'
                warnings.warn(msg)
                break

            print(p, round(new_distance, 3), '\n ============', e)

            # 将新的support点插入多边形中(将旧的边e分割为新的两条边)
            poly.insert_edge_point(e, p)

            points = get_sorted_points(poly.points)
            plot_polygon(points)

        return ret